/* ******************************************** */
/* * Robot Player / Stage Interface           * */
/* * Authors: André Bannwart Perina           * */
/* *          Antonio Canabrava Fraideinberze * */
/* *          Willian Felipe Lopes da Silva   * */
/* ******************************************** */

#include <errno.h>
#include <float.h>
#include <limits.h>
#include <math.h>
#include <libplayerc/playerc.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <unistd.h>

#include "../include/rutils.h"

/* Loop multiplier (the loop runs ITER_MULTIPLIER times faster than the number of genes) */
#define ITER_MULTIPLIER 10

/**
 * Algorithm that simulates the robot, getting as input the destination and
 * the delta files.
 */
int main(int argc, const char **argv) {
	if(argc != 9) {
		fprintf(stderr, "Usage: %s playerPort inputFile outputFile stopDist movTol destTol xDest yDest\n", argv[0]);
		return EXIT_FAILURE;
	}

	/* Auxiliar Variable */
	int i;
	/* Counter used to halt when robot crashes */
	int count = 0;
	/* Robot/Player Port */
	int playerPort = atoi(argv[1]);
	/* Number of genes */
	int deltaSize;
	
	/* Vector of genes */
	double *delta;
	/* Obstacle avoidance distance */
	double stopDist = atof(argv[4]);
	/* Tolerance of each turning/moving operation (e = 0 is time consuming) */
	double e = atof(argv[5]);
	/* Tolerance radius of reaching destination */
	double eDest = atof(argv[6]);
	/* Previous Position */
	double prev_pos[3];
	/* Actual Position */
	double actu_pos[3];
	/* x coordinate of destination */
	double xDest = atof(argv[7]);
	/* y coordinate of destination */
	double yDest = atof(argv[8]);
	/* Distance from actual position to destination */
	double dDest;
	/* Minimum distance reached at the end of execution */
	double dFinal = DBL_MAX;

	FILE *deltaFile, *fitnessFile;

	/* Variables used to calculate elapsed time */
	time_t tFinal;
	time_t init;

	/* Player variables */
	playerc_client_t *client;
	playerc_position2d_t *device;
	playerc_laser_t *laser;

	/* Open Delta File */
	if(NULL == (deltaFile = fopen(argv[2], "r"))) {
		fprintf(stderr, "%s: %s: %s\n", argv[0], strerror(errno), argv[2]);
		return EXIT_FAILURE;
	}

	/* Get all genes */
	fscanf(deltaFile, "%d", &deltaSize);
	delta = (double *) malloc(deltaSize*sizeof(double));
	for(i = 0; i < deltaSize; i++)
		fscanf(deltaFile, "%lf", &delta[i]);
	fclose(deltaFile);

	/* Initialise robot, laser and position devices */
	client = playerc_client_create(NULL, "localhost", playerPort);
	if (0 != playerc_client_connect(client))
		return -1;
	device = playerc_position2d_create(client, 0);
	if(playerc_position2d_subscribe(device, PLAYERC_OPEN_MODE))
		return -1;
	laser = playerc_laser_create(client, 0);
	if(playerc_laser_subscribe(laser, PLAYERC_OPEN_MODE))
		return -1;

	/* Wait for 1 complete turn of the laser */
	while(laser->scan_count < 360)
		playerc_client_read(client);

	/* Get initial time */
	init = time(NULL);

	for(i = 0; i < deltaSize*ITER_MULTIPLIER; i++) {
		/* Get current data from robot */
		playerc_client_read(client);

		/* Update positions */
		actu_pos[0] = device->px;
		actu_pos[1] = device->py;
		actu_pos[2] = device->pa;

		/* Update final distance and time values */
		if((dDest = pow(actu_pos[0] - xDest, 2) + pow(actu_pos[1] - yDest, 2)) < dFinal)
			dFinal = dDest;
		tFinal = time(NULL) - init;

		/* If robot is near enough the goal, finish execution */
		if(dFinal < eDest)
		    break;

		/* If robot is stuck, start counting */
		if(fabs(actu_pos[0] - prev_pos[0]) < e && 
		   fabs(actu_pos[1] - prev_pos[1]) < e && 
                   fabs(actu_pos[2] - prev_pos[2]) < e)
			count++;
		else
			count = 0;

		/* If counting has reached 10, halt execution (robot is deadlocked) */
		if(10 == count) {
			tFinal = -1;
		    break;
		}

		/* Update iteration positions */
		prev_pos[0] = actu_pos[0];
		prev_pos[1] = actu_pos[1];
		prev_pos[2] = actu_pos[2];

		/* Get current data from robot */
		playerc_client_read(client);

		/* If the robot is getting close to any obstacle, start avoidance routine */
		if(laser->scan[180][0] <= stopDist) {
			/* Stop the robot */
			playerc_position2d_set_cmd_vel(device, 0, 0, 0, 1);

			/* Get the minimal distance from the robot to any obstacle (and its angle) */
			int j, minAngle;
			double minDist = DBL_MAX;
			for(j = 0, minAngle = 0; j < 360; j++) {
				if(laser->scan[j][0] < minDist) {
					minDist = laser->scan[j][0];
					minAngle = j;
				}
			}

			/* Make the robot heading parallel to the obstacle */
			if(minAngle)
				rutils_rotate_rel_blk(client, device, minAngle * (M_PI/360.0), 10, e);
			/* If the robot is facing the obstacle, activate rear movement */
			else
				playerc_position2d_set_cmd_car(device, -0.5, 0);

			/* Keep repeating until the heading is free from obstacles */
			continue;
		}
	    
		/* Change the robot's direction based on the genes */
		if(!(i % ITER_MULTIPLIER))
			playerc_position2d_set_cmd_car(device, 0.5, delta[i/ITER_MULTIPLIER]);
	}

	/* Destroy allocated player stuff */
	playerc_laser_unsubscribe(laser);
	playerc_laser_destroy(laser);
	playerc_position2d_unsubscribe(device);
	playerc_position2d_destroy(device);
	playerc_client_disconnect(client);
	playerc_client_destroy(client);

	/* Open fitness file */
	if(NULL == (fitnessFile = fopen(argv[3], "w"))) {
		fprintf(stderr, "%s: %s: %s\n", argv[0], strerror(errno), argv[3]);
		return EXIT_FAILURE;
	}

	/* Save fitness values */
	fprintf(fitnessFile, "%d %lf\n", tFinal, dFinal);
	fclose(fitnessFile);

	free(delta);

	return EXIT_SUCCESS;
}
